﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace ARDrone2
{
    /// <summary>
    /// Actions of the AR.Drone. (Through UDP channel)
    /// </summary>
    public static class Action
    {
        /// <summary>
        /// Convert floating-point parameter to formatted string.
        /// </summary>
        /// <param name="param">Floating-point parameter to convert.</param>
        /// <returns>The string representing the floating-point parameter.</returns>
        /// <exception cref="System.ArgumentNullException"></exception>
        private static string _FloatingPointParam(float param)
        {
            if (param == null) throw new ArgumentNullException("param", "The argument cannot be null.");
            return (BitConverter.ToInt32(BitConverter.GetBytes(param), 0)).ToString();
        }

        /// <summary>
        /// Take off from the ground to air.
        /// </summary>
        /// <remarks>
        /// This command should be repeated until the drone state in the navdata 
        /// shows that drone actually took off. If no other command is supplied, 
        /// the drone enters a hovering mode and stays still at approximately 
        /// 1 meter above ground.
        /// </remarks>
        /// <exception cref="System.Net.Sockets.SocketException"></exception>
        public static void TakeOff()
        {
            // Construct the command line
            string cmdstr = "AT*REF=" + CommunicationChannel.GetNextATCommandSequenceNumber().ToString() + ",290718208\r";

            // Send the command
            CommunicationChannel.SendATCommand(cmdstr);
        }

        /// <summary>
        /// Land from the air to ground.
        /// </summary>
        /// <remarks>
        /// This command should be repeated until the drone state in the navdata 
        /// shows that drone actually landed, and should be sent as a safety 
        /// whenever an abnormal situation is detected.
        /// </remarks>
        /// <exception cref="System.Net.Sockets.SocketException"></exception>
        public static void Land()
        {
            // Construct the command line
            string cmdstr = "AT*REF=" + CommunicationChannel.GetNextATCommandSequenceNumber().ToString() + ",290717696\r";

            // Send the command
            CommunicationChannel.SendATCommand(cmdstr);
        }

        /// <summary>
        /// Switch the drone emergency mode between emergent and normal states.
        /// </summary>
        /// <remarks>
        /// When the drone is a "normal" state (flying or waiting on the ground), 
        /// sending this command makes the drone enter an emergency mode. Engines 
        /// are cut-off no matter the drone state. (ie. the drone crashes, potentially 
        /// violently).
        /// When the drone is in an emergency mode (following a previous emergency 
        /// order or a crash), sending this command makes the drone resume to a normal 
        /// state (allowing it to take-off again), at the condition the cause of 
        /// the emergency was solved.
        /// </remarks>
        /// <exception cref="System.Net.Sockets.SocketException"></exception>
        public static void SwitchEmergencyMode()
        {
            // Construct the command line
            string cmdstr = "AT*REF=" + CommunicationChannel.GetNextATCommandSequenceNumber().ToString() + ",290717952\r";

            // Send the command
            CommunicationChannel.SendATCommand(cmdstr);
        }

        /// <summary>
        /// Movement actions of the AR.Drone.
        /// </summary>
        public static class Move
        {
            /// <summary>
            /// Make the drone move in advanced progress mode.
            /// </summary>
            /// <param name="enable">Enable progress. (Set true to enable progress, and false to make it hover still.)</param>
            /// <param name="rollPerc">Left-right tilt percentage of the maximum configuration. (Within range [-1..1]. Set negative to tilt left, and positive right.)</param>
            /// <param name="pitchPerc">Front-back tilt percentage of the maximum configuration. (Within range [-1..1]. Set negative to lower its nose, and positive to raise.)</param>
            /// <param name="vertSpeedPerc">Vertical speed percentage of the maximum configuration. (Within range [-1..1]. Set positive to raise the drone, and negative to descent.)</param>
            /// <param name="yawSpeedPerc">Yaw angular speed percentage of the maximum configuration. (Within range [-1..1]. Set negative to spin left, and positive right.)</param>
            /// <param name="combYaw">Enable combined yaw mode.</param>
            /// <param name="absCtrl">Enable absolute control mode. (Without stopping inertial drifting.)</param>
            /// <param name="magCtrl">Enable geomagnetic control. (Set true only if geomegnetic control is necessary.)</param>
            /// <param name="magPsiPrec">Normalized psi angle from north. (Within range [-1..1]. Set positive to orient east, and negative west.)</param>
            /// <param name="magPsiAccurPrec">Maximum deviation from the actual geomagnetic heading. (Within range [-1..1]. Negative values indicates invalid heading.)</param>
            /// <exception cref="System.ArgumentOutOfRangeException"></exception>
            /// <exception cref="System.Net.Sockets.SocketException"></exception>
            public static void AdvancedProgress(
                bool enable = false, // Arg 2
                float rollPerc = 0, // Arg 3
                float pitchPerc = 0, // Arg 4
                float vertSpeedPerc = 0, // Arg 5
                float yawSpeedPerc = 0, // Arg 6
                bool combYaw = false, // Arg 2 
                bool absCtrl = false, // Arg 2
                bool magCtrl = false, // (en: Arg 7, Arg 8)
                float magPsiPrec = 0, // Arg 7
                float magPsiAccurPrec = 0.9f // Arg 8
                )
            {
                // Check parameters
                if (rollPerc < -1.0f || rollPerc > 1.0f) throw new ArgumentOutOfRangeException("rollPrec", "The available range is [-1..1].");
                if (pitchPerc < -1.0f || pitchPerc > 1.0f) throw new ArgumentOutOfRangeException("pitchPrec", "The available range is [-1..1].");
                if (vertSpeedPerc < -1.0f || vertSpeedPerc > 1.0f) throw new ArgumentOutOfRangeException("vertSpeedPrec", "The available range is [-1..1].");
                if (yawSpeedPerc < -1.0f || yawSpeedPerc > 1.0f) throw new ArgumentOutOfRangeException("yawSpeedPrec", "The available range is [-1..1].");
                if (magPsiPrec < -1.0f || magPsiPrec > 1.0f) throw new ArgumentOutOfRangeException("msgPsiPrec", "The available range is [-1..1].");
                if (magPsiAccurPrec < -1.0f || magPsiAccurPrec > 1.0f) throw new ArgumentOutOfRangeException("magPsiAccurPrec", "The available range is [-1..1].");


                StringBuilder cmdBuilder = new StringBuilder(64);
                int enableBits = 0;

                // Select the AT command type
                if (magCtrl) cmdBuilder.Append("AT*PCMD_MAG="); // Geomagnetic control enabled
                else cmdBuilder.Append("AT*PCMD="); // Geomagnetic control disabled

                // Append the sequence number (Arg 1)
                cmdBuilder.Append(CommunicationChannel.GetNextATCommandSequenceNumber());
                cmdBuilder.Append(",");

                // Append the enable bit-field (Arg 2)
                if (enable) enableBits += 1;
                if (combYaw) enableBits += 2;
                if (absCtrl) enableBits += 4;
                cmdBuilder.Append(enableBits);
                cmdBuilder.Append(",");

                // Append the parameters except magnetic ones (Arg 3,..,6)
                cmdBuilder.Append(_FloatingPointParam(rollPerc) + ",");
                cmdBuilder.Append(_FloatingPointParam(pitchPerc) + ",");
                cmdBuilder.Append(_FloatingPointParam(vertSpeedPerc) + ",");
                cmdBuilder.Append(_FloatingPointParam(yawSpeedPerc));

                // Append the magnetic control parameters if selected (Arg 7, 8)
                if (magCtrl)
                {
                    cmdBuilder.Append("," + _FloatingPointParam(magPsiPrec));
                    cmdBuilder.Append("," + _FloatingPointParam(magPsiAccurPrec));
                }

                // Send the command
                CommunicationChannel.SendATCommand(cmdBuilder.ToString() + "\r");
            }

            public static void Hover()
            {
                // Make the drone hover
                AdvancedProgress(false);
            }
            
            public static void Tilt(float rollPerc, float pitchPerc)
            {
                // Check parameters
                if (rollPerc < -1.0f || rollPerc > 1.0f) throw new ArgumentOutOfRangeException("rollPrec", "The available range is [-1..1].");
                if (pitchPerc < -1.0f || pitchPerc > 1.0f) throw new ArgumentOutOfRangeException("pitchPrec", "The available range is [-1..1].");

                // Progress
                AdvancedProgress(true, rollPerc, pitchPerc);
            }
            public static void MoveFront(float perc)
            {
                // Check parameters
                if (perc < 0 || perc > 1.0f) throw new ArgumentOutOfRangeException("perc", "The available range is [0..1].");

                // Progress
                AdvancedProgress(true, 0, -perc);
            }
            public static void MoveBack(float perc)
            {
                // Check parameters
                if (perc < 0 || perc > 1.0f) throw new ArgumentOutOfRangeException("perc", "The available range is [0..1].");

                // Progress
                AdvancedProgress(true, 0, perc);
            }
            public static void MoveLeft(float perc)
            {
                // Check parameters
                if (perc < 0 || perc > 1.0f) throw new ArgumentOutOfRangeException("perc", "The available range is [0..1].");

                // Progress
                AdvancedProgress(true, -perc, 0);
            }
            public static void MoveRight(float perc)
            {
                // Check parameters
                if (perc < 0 || perc > 1.0f) throw new ArgumentOutOfRangeException("perc", "The available range is [0..1].");

                // Progress
                AdvancedProgress(true, perc, 0);
            }

            public static void AlterVerticalSpeed(float vertSpeedPerc)
            {
                // Check parameters
                if (vertSpeedPerc < -1.0f || vertSpeedPerc > 1.0f) throw new ArgumentOutOfRangeException("vertSpeedPerc", "The available range is [-1..1].");

                // Progress
                AdvancedProgress(true, 0, 0, vertSpeedPerc);
            }
            public static void Lift(float speedPerc)
            {
                // Check parameters
                if (speedPerc < 0 || speedPerc > 1.0f) throw new ArgumentOutOfRangeException("speedPerc", "The available range is [0..1].");

                // Progress
                AdvancedProgress(true, 0, 0, speedPerc);
            }
            public static void Descent(float speedPerc)
            {
                // Check parameters
                if (speedPerc < 0 || speedPerc > 1.0f) throw new ArgumentOutOfRangeException("speedPerc", "The available range is [0..1].");

                // Progress
                AdvancedProgress(true, 0, 0, -speedPerc);
            }

            public static void Spin(float yawSpeedPerc)
            {
                // Check parameters
                if (yawSpeedPerc < -1.0f || yawSpeedPerc > 1.0f) throw new ArgumentOutOfRangeException("yawSpeedPerc", "The available range is [-1..1].");

                // Progress
                AdvancedProgress(true, 0, 0, 0, yawSpeedPerc);
            }
            public static void SpinLeft(float speedPerc)
            {
                // Check parameters
                if (speedPerc < -1.0f || speedPerc > 1.0f) throw new ArgumentOutOfRangeException("speedPerc", "The available range is [0..1].");

                // Progress
                AdvancedProgress(true, 0, 0, 0, -speedPerc);
            }
            public static void SpinRight(float speedPerc)
            {
                // Check parameters
                if (speedPerc < -1.0f || speedPerc > 1.0f) throw new ArgumentOutOfRangeException("speedPerc", "The available range is [0..1].");

                // Progress
                AdvancedProgress(true, 0, 0, 0, speedPerc);
            }

            public static void OrientGeomagnetic(float psiPerc, float psiAccurPerc)
            {
                // Check parameters
                if (psiPerc < -1.0f || psiPerc > 1.0f) throw new ArgumentOutOfRangeException("psiPerc", "The available range is [-1..1].");
                if (psiAccurPerc < -1.0f || psiAccurPerc > 1.0f) throw new ArgumentOutOfRangeException("psiAccurPerc", "The available range is [-1..1].");

                // Progress
                AdvancedProgress(true, 0, 0, 0, 0, false, false, true, psiPerc, psiAccurPerc);
            }
        }
    }
}
